#include "utils.h"
#include "ASSteeringBehaviors.h"
#include "ASEntity.h"
#include "ASMobileComponent.h"
#include "Transformations.h"

#include "geometry.h"
#include "ASWall2D.h"

NS_AS_BEGIN

#define kASSteerBehaviors_WeightSeparation			50.0
#define kASSteerBehaviors_WeightCohesion			1.0
#define kASSteerBehaviors_WeightAlignment			1.0
#define kASSteerBehaviors_WeightWander				1.0
#define kASSteerBehaviors_WeightObstacleAvoidance	1.0
#define kASSteerBehaviors_WeightWallAvoidance		1.0
#define kASSteerBehaviors_WeightSeek				1.0
#define kASSteerBehaviors_WeightFlee				1.0
#define kASSteerBehaviors_WeightArrive				1.0
#define kASSteerBehaviors_WeightPursuit				1.0
#define kASSteerBehaviors_WeightOffsetPursuit		1.0
#define kASSteerBehaviors_WeightInterpose			1.0
#define kASSteerBehaviors_WeightHide				1.0
#define kASSteerBehaviors_WeightEvade				1.0
#define kASSteerBehaviors_WeightFollowPath			1.0
#define kASSteerBehaviors_WeightForwardResistance   0.1
#define kASSteerBehaviors_WeightApproach			1.0

	//------------------------- ctor -----------------------------------------
	//
	//------------------------------------------------------------------------
	ASSteeringBehavior::ASSteeringBehavior(ASMobileComponent* agent):
	m_pMobile(agent),
	m_dWallDetectionFeelerLength(Prm.WallDetectionFeelerLength),
	m_Feelers(3),
	m_Deceleration(normal),
	m_pTargetAgent1(nullptr),
	m_pTargetAgent2(nullptr),
	m_dWaypointSeekDistSq(WaypointSeekDist*WaypointSeekDist),
	m_iApproachRange(0),
	m_iFlags(0)
{
	//stuff for the wander behavior
	double theta = RandFloat() * TwoPi;

	//create a vector to a target position on the wander circle
	//m_vWanderTarget = Vector2D(m_dWanderRadius * cos(theta),m_dWanderRadius * sin(theta));

	//create a Path
	m_pPath = new ASPath();
	//m_pPath->LoopOn();

}

//---------------------------------dtor ----------------------------------
ASSteeringBehavior::~ASSteeringBehavior(){delete m_pPath;}


/////////////////////////////////////////////////////////////////////////////// CALCULATE METHODS 


//----------------------- Calculate --------------------------------------
//
//  calculates the accumulated steering force according to the method set
//  in m_SummingMethod
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::Calculate()
//{ 
//	//reset the steering force
//	m_vSteeringForce.Zero();
//
//	//use space partitioning to calculate the neighbours of this vehicle
//	//if switched on. If not, use the standard tagging system
//	//if (!isSpacePartitioningOn())
//	//{
//	//tag neighbors if any of the following 3 group behaviors are switched on
//	if (On(separation) || On(allignment) || On(cohesion))
//	{
//		m_pMobile->getMobileControler()->tagMobileWithinViewRange(m_pMobile->getHolder(),m_pMobile->getHolder()->getBoundRadius());
//		//m_pMobile->World()->TagVehiclesWithinViewRange(m_pMobile, m_dViewDistance);
//	}
//	//}
//	//else
//	//{
//	//  //calculate neighbours in cell-space if any of the following 3 group
//	//  //behaviors are switched on
//	//  if (On(separation) || On(allignment) || On(cohesion))
//	//  {
//	//    m_pMobile->World()->CellSpace()->CalculateNeighbors(m_pMobile->Pos(), m_dViewDistance);
//	//  }
//	//}
//
//	switch (m_SummingMethod)
//	{
//	case weighted_average:
//
//		m_vSteeringForce = CalculateWeightedSum(); break;
//
//	case prioritized:
//
//		m_vSteeringForce = CalculatePrioritized(); break;
//
//	case dithered:
//
//		m_vSteeringForce = CalculateDithered();break;
//
//	default:m_vSteeringForce = Vector2D(0,0); 
//
//	}//end switch
//	//m_vSteeringForce.Accuracy(AS_VECTOR_ACCURACY);
//	return m_vSteeringForce;
//}
Vector2D ASSteeringBehavior::Calculate(float dt)
{ 
	//reset the steering force
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::CrowdPath Holder is NULL");

	m_vSteeringForce.Zero();
	ASAssert(m_pMobile->getMobileControler(),"m_pMobile->getMobileControler() is null");
	if (On(separation)||On(crowd_path))
	{
		m_pMobile->getMobileControler()->tagMobileWithinViewRange(pHolder,pHolder->getBoundRadius());
	}

	//Vector2D force;

	if (On(wall_avoidance))
	{
		m_vSteeringForce += WallAvoidance(m_pMobile->getMobileControler()->getWalls()) * kASSteerBehaviors_WeightWallAvoidance;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}

	if (On(obstacle_avoidance))
	{
		m_vSteeringForce += ObstacleAvoidance(m_pMobile->getMobileControler()->getObstacles()) * kASSteerBehaviors_WeightObstacleAvoidance;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}

	if (On(separation))
	{
		m_vSteeringForce += Separation(m_pMobile->getMobileControler()->getMobiles()) * kASSteerBehaviors_WeightSeparation;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}

	if (On(crowd_path))
	{
		m_vSteeringForce += CrowdPath(m_pMobile->getMobileControler()->getMobiles(),dt) * kASSteerBehaviors_WeightFollowPath;

	}



	if (On(approach))
	{
		ASAssert(m_pTargetAgent1,"approach target is null");
		m_vSteeringForce += Approach(m_pTargetAgent1,m_iApproachRange) * kASSteerBehaviors_WeightApproach;
	}
	else if (On(approach_horizon))
	{
		ASAssert(m_pTargetAgent1,"approach_horizon target is null");
		m_vSteeringForce += ApproachHorizon(m_pTargetAgent1,m_iApproachRange,dt) * kASSteerBehaviors_WeightApproach;
	}

	if (On(seek))
	{
		m_vSteeringForce += Seek(m_pMobile->getAim()) * kASSteerBehaviors_WeightSeek;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}

	if (On(arrive))
	{
		m_vSteeringForce += Arrive(m_pMobile->getAim(), m_Deceleration) * kASSteerBehaviors_WeightArrive;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}

	if (On(offset_pursuit))
	{
		ASAssert (m_pTargetAgent1 , "pursuit target not assigned");
		ASAssert (!m_vOffset.isZero() , "No offset assigned");

		m_vSteeringForce += OffsetPursuit(m_pTargetAgent1, m_vOffset) * kASSteerBehaviors_WeightOffsetPursuit;

		//if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
	}


	m_vSteeringForce.Truncate(pHolder->getMaxForce());

	return m_vSteeringForce;
}

//------------------------- ForwardComponent -----------------------------
//
//  returns the forward oomponent of the steering force
//------------------------------------------------------------------------
double ASSteeringBehavior::ForwardComponent()
{
	return m_pMobile->getHolder()->getHeading().Dot(m_vSteeringForce);
}

//--------------------------- SideComponent ------------------------------
//  returns the side component of the steering force
//------------------------------------------------------------------------
double ASSteeringBehavior::SideComponent()
{
	return m_pMobile->getHolder()->getSide().Dot(m_vSteeringForce);
}


//--------------------- AccumulateForce ----------------------------------
//
//  This function calculates how much of its max steering force the 
//  vehicle has left to apply and then applies that amount of the
//  force to add.
//	RunningTot is in/out param
//------------------------------------------------------------------------
bool ASSteeringBehavior::AccumulateForce(Vector2D& RunningTot,const Vector2D& ForceToAdd,double MaxForce)
{
	//calculate how much steering force the vehicle has used so far
	double MagnitudeSoFar = RunningTot.Length();

	//calculate how much steering force remains to be used by this vehicle
	double MagnitudeRemaining = MaxForce - MagnitudeSoFar;

	//return false if there is no more force left to use
	if (MagnitudeRemaining <= 0.0) return false;

	//calculate the magnitude of the force we want to add
	double MagnitudeToAdd = ForceToAdd.Length();

	//if the magnitude of the sum of ForceToAdd and the running total
	//does not exceed the maximum force available to this vehicle, just
	//add together. Otherwise add as much of the ForceToAdd vector is
	//possible without going over the max.
	if (MagnitudeToAdd < MagnitudeRemaining)
	{
		RunningTot += ForceToAdd;
	}

	else
	{
		//add it to the steering force
		RunningTot += (Vec2DNormalize(ForceToAdd) * MagnitudeRemaining); 
	}

	return true;
}



//---------------------- CalculatePrioritized ----------------------------
//
//  this method calls each active steering behavior in order of priority
//  and acumulates their forces until the max steering force magnitude
//  is reached, at which time the function returns the steering force 
//  accumulated to that  point
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::CalculatePrioritized()
//{       
//	Vector2D force;
//
//	if (On(wall_avoidance))
//	{
//		force = WallAvoidance(m_pMobile->getMobileControler()->getWalls()) *
//			m_dWeightWallAvoidance;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(obstacle_avoidance))
//	{
//		ASAssert(m_pMobile->getMobileControler(),"m_pMobile->getMobileControler() is null");
//		force = ObstacleAvoidance(m_pMobile->getMobileControler()->getObstacles()) * 
//			m_dWeightObstacleAvoidance;
//		
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(evade))
//	{
//		assert(m_pTargetAgent1 && "Evade target not assigned");
//
//		force = Evade(m_pTargetAgent1) * m_dWeightEvade;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//
//	if (On(flee))
//	{
//		force = Flee(m_pMobile->getAim()) * m_dWeightFlee;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//
//
//	//these next three can be combined for flocking behavior (wander is
//	//also a good behavior to add into this mix)
//	//if (!isSpacePartitioningOn())
//	//{
//	if (On(separation))
//	{
//		force = Separation(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightSeparation;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(allignment))
//	{
//		force = Alignment(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightAlignment;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(cohesion))
//	{
//		force = Cohesion(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightCohesion;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//	//}
//
//	//else
//	//{
//
//	//  if (On(separation))
//	//  {
//	//    force = SeparationPlus(m_pMobile->World()->Agents()) * m_dWeightSeparation;
//
//	//    if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	//  }
//
//	//  if (On(allignment))
//	//  {
//	//    force = AlignmentPlus(m_pMobile->World()->Agents()) * m_dWeightAlignment;
//
//	//    if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	//  }
//
//	//  if (On(cohesion))
//	//  {
//	//    force = CohesionPlus(m_pMobile->World()->Agents()) * m_dWeightCohesion;
//
//	//    if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	//  }
//	//}
//
//	if (On(seek))
//	{
//		force = Seek(m_pMobile->getAim()) * m_dWeightSeek;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//
//	if (On(arrive))
//	{
//		force = Arrive(m_pMobile->getAim(), m_Deceleration) * m_dWeightArrive;
//		
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	//if (On(wander))
//	//{
//	//  force = Wander() * m_dWeightWander;
//
//	//  if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	//}
//
//	if (On(pursuit))
//	{
//		assert(m_pTargetAgent1 && "pursuit target not assigned");
//
//		force = Pursuit(m_pTargetAgent1) * m_dWeightPursuit;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(offset_pursuit))
//	{
//		assert (m_pTargetAgent1 && "pursuit target not assigned");
//		assert (!m_vOffset.isZero() && "No offset assigned");
//
//		force = OffsetPursuit(m_pTargetAgent1, m_vOffset);
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(interpose))
//	{
//		assert (m_pTargetAgent1 && m_pTargetAgent2 && "Interpose agents not assigned");
//
//		force = Interpose(m_pTargetAgent1, m_pTargetAgent2) * m_dWeightInterpose;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	if (On(hide))
//	{
//		assert(m_pTargetAgent1 && "Hide target not assigned");
//
//		force = Hide(m_pTargetAgent1, m_pMobile->getMobileControler()->getObstacles()) * m_dWeightHide;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//
//	if (On(follow_path))
//	{
//		force = FollowPath() * m_dWeightFollowPath;
//
//		if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
//	}
//
//	return m_vSteeringForce;
//}


//---------------------- CalculateWeightedSum ----------------------------
//
//  this simply sums up all the active behaviors X their weights and 
//  truncates the result to the max available steering force before 
//  returning
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::CalculateWeightedSum()
//{        
//	if (On(wall_avoidance))
//	{
//		m_vSteeringForce += WallAvoidance(m_pMobile->getMobileControler()->getWalls()) *
//			m_dWeightWallAvoidance;
//	}
//
//	if (On(obstacle_avoidance))
//	{
//		m_vSteeringForce += ObstacleAvoidance(m_pMobile->getMobileControler()->getObstacles()) * 
//			m_dWeightObstacleAvoidance;
//	}
//
//	if (On(evade))
//	{
//		assert(m_pTargetAgent1 && "Evade target not assigned");
//
//		m_vSteeringForce += Evade(m_pTargetAgent1) * m_dWeightEvade;
//	}
//
//
//	//these next three can be combined for flocking behavior (wander is
//	//also a good behavior to add into this mix)
//	//if (!isSpacePartitioningOn())
//	//{
//	if (On(separation))
//	{
//		m_vSteeringForce += Separation(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightSeparation;
//	}
//
//	if (On(allignment))
//	{
//		m_vSteeringForce += Alignment(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightAlignment;
//	}
//
//	if (On(cohesion))
//	{
//		m_vSteeringForce += Cohesion(m_pMobile->getMobileControler()->getMobiles()) * m_dWeightCohesion;
//	}
//	//}
//	//else
//	//{
//	//  if (On(separation))
//	//  {
//	//    m_vSteeringForce += SeparationPlus(m_pMobile->World()->Agents()) * m_dWeightSeparation;
//	//  }
//
//	//  if (On(allignment))
//	//  {
//	//    m_vSteeringForce += AlignmentPlus(m_pMobile->World()->Agents()) * m_dWeightAlignment;
//	//  }
//
//	//  if (On(cohesion))
//	//  {
//	//    m_vSteeringForce += CohesionPlus(m_pMobile->World()->Agents()) * m_dWeightCohesion;
//	//  }
//	//}
//
//
//	//if (On(wander))
//	//{
//	//  m_vSteeringForce += Wander() * m_dWeightWander;
//	//}
//
//	if (On(seek))
//	{
//		m_vSteeringForce += Seek(m_pMobile->getAim()) * m_dWeightSeek;
//	}
//
//	if (On(flee))
//	{
//		m_vSteeringForce += Flee(m_pMobile->getAim()) * m_dWeightFlee;
//	}
//
//	if (On(arrive))
//	{
//		m_vSteeringForce += Arrive(m_pMobile->getAim(), m_Deceleration) * m_dWeightArrive;
//	}
//
//	if (On(pursuit))
//	{
//		assert(m_pTargetAgent1 && "pursuit target not assigned");
//
//		m_vSteeringForce += Pursuit(m_pTargetAgent1) * m_dWeightPursuit;
//	}
//
//	if (On(offset_pursuit))
//	{
//		assert (m_pTargetAgent1 && "pursuit target not assigned");
//		assert (!m_vOffset.isZero() && "No offset assigned");
//
//		m_vSteeringForce += OffsetPursuit(m_pTargetAgent1, m_vOffset) * m_dWeightOffsetPursuit;
//	}
//
//	if (On(interpose))
//	{
//		assert (m_pTargetAgent1 && m_pTargetAgent2 && "Interpose agents not assigned");
//
//		m_vSteeringForce += Interpose(m_pTargetAgent1, m_pTargetAgent2) * m_dWeightInterpose;
//	}
//
//	if (On(hide))
//	{
//		assert(m_pTargetAgent1 && "Hide target not assigned");
//
//		m_vSteeringForce += Hide(m_pTargetAgent1, m_pMobile->getMobileControler()->getObstacles()) * m_dWeightHide;
//	}
//
//	if (On(follow_path))
//	{
//		m_vSteeringForce += FollowPath() * m_dWeightFollowPath;
//	}
//
//	m_vSteeringForce.Truncate(m_pMobile->getMaxForce());
//
//	return m_vSteeringForce;
//}


//---------------------- CalculateDithered ----------------------------
//
//  this method sums up the active behaviors by assigning a probabilty
//  of being calculated to each behavior. It then tests the first priority
//  to see if it should be calcukated this simulation-step. If so, it
//  calculates the steering force resulting from this behavior. If it is
//  more than zero it returns the force. If zero, or if the behavior is
//  skipped it continues onto the next priority, and so on.
//
//  NOTE: Not all of the behaviors have been implemented in this method,
//        just a few, so you get the general idea
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::CalculateDithered()
//{  
//	//reset the steering force
//	m_vSteeringForce.Zero();
//
//	if (On(wall_avoidance) && RandFloat() < Prm.prWallAvoidance)
//	{
//		m_vSteeringForce = WallAvoidance(m_pMobile->getMobileControler()->getWalls()) *
//			m_dWeightWallAvoidance / Prm.prWallAvoidance;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	if (On(obstacle_avoidance) && RandFloat() < Prm.prObstacleAvoidance)
//	{
//		m_vSteeringForce += ObstacleAvoidance(m_pMobile->getMobileControler()->getObstacles()) * 
//			m_dWeightObstacleAvoidance / Prm.prObstacleAvoidance;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	//if (!isSpacePartitioningOn())
//	//{
//	if (On(separation) && RandFloat() < Prm.prSeparation)
//	{
//		m_vSteeringForce += Separation(m_pMobile->getMobileControler()->getMobiles()) * 
//			m_dWeightSeparation / Prm.prSeparation;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//	//}
//	//else
//	//{
//	//  if (On(separation) && RandFloat() < Prm.prSeparation)
//	//  {
//	//    m_vSteeringForce += SeparationPlus(m_pMobile->World()->Agents()) * 
//	//                        m_dWeightSeparation / Prm.prSeparation;
//
//	//    if (!m_vSteeringForce.isZero())
//	//    {
//	//      m_vSteeringForce.Truncate(m_pMobile->MaxForce()); 
//	//    
//	//      return m_vSteeringForce;
//	//    }
//	//  }
//	//}
//
//
//	if (On(flee) && RandFloat() < Prm.prFlee)
//	{
//		m_vSteeringForce += Flee(m_pMobile->getAim()) * m_dWeightFlee / Prm.prFlee;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	if (On(evade) && RandFloat() < Prm.prEvade)
//	{
//		assert(m_pTargetAgent1 && "Evade target not assigned");
//
//		m_vSteeringForce += Evade(m_pTargetAgent1) * m_dWeightEvade / Prm.prEvade;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//
//	//if (!isSpacePartitioningOn())
//	//{
//	if (On(allignment) && RandFloat() < Prm.prAlignment)
//	{
//		m_vSteeringForce += Alignment(m_pMobile->getMobileControler()->getMobiles()) *
//			m_dWeightAlignment / Prm.prAlignment;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	if (On(cohesion) && RandFloat() < Prm.prCohesion)
//	{
//		m_vSteeringForce += Cohesion(m_pMobile->getMobileControler()->getMobiles()) * 
//			m_dWeightCohesion / Prm.prCohesion;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//	//}
//	//else
//	//{
//	//  if (On(allignment) && RandFloat() < Prm.prAlignment)
//	//  {
//	//    m_vSteeringForce += AlignmentPlus(m_pMobile->World()->Agents()) *
//	//                        m_dWeightAlignment / Prm.prAlignment;
//
//	//    if (!m_vSteeringForce.isZero())
//	//    {
//	//      m_vSteeringForce.Truncate(m_pMobile->MaxForce()); 
//	//    
//	//      return m_vSteeringForce;
//	//    }
//	//  }
//
//	//  if (On(cohesion) && RandFloat() < Prm.prCohesion)
//	//  {
//	//    m_vSteeringForce += CohesionPlus(m_pMobile->World()->Agents()) *
//	//                        m_dWeightCohesion / Prm.prCohesion;
//
//	//    if (!m_vSteeringForce.isZero())
//	//    {
//	//      m_vSteeringForce.Truncate(m_pMobile->MaxForce()); 
//	//    
//	//      return m_vSteeringForce;
//	//    }
//	//  }
//	//}
//
//	//if (On(wander) && RandFloat() < Prm.prWander)
//	//{
//	//  m_vSteeringForce += Wander() * m_dWeightWander / Prm.prWander;
//
//	//  if (!m_vSteeringForce.isZero())
//	//  {
//	//    m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//	//    
//	//    return m_vSteeringForce;
//	//  }
//	//}
//
//	if (On(seek) && RandFloat() < Prm.prSeek)
//	{
//		m_vSteeringForce += Seek(m_pMobile->getAim()) * m_dWeightSeek / Prm.prSeek;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	if (On(arrive) && RandFloat() < Prm.prArrive)
//	{
//		m_vSteeringForce += Arrive(m_pMobile->getAim(), m_Deceleration) * 
//			m_dWeightArrive / Prm.prArrive;
//
//		if (!m_vSteeringForce.isZero())
//		{
//			m_vSteeringForce.Truncate(m_pMobile->getMaxForce()); 
//
//			return m_vSteeringForce;
//		}
//	}
//
//	return m_vSteeringForce;
//}



/////////////////////////////////////////////////////////////////////////////// START OF BEHAVIORS

//------------------------------- Seek -----------------------------------
//
//  Given a target, this behavior returns a steering force which will
//  direct the agent towards the target
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Seek(Vector2D TargetPos)
{
	Vector2D DesiredVelocity = Vec2DNormalize(TargetPos - m_pMobile->getHolder()->getPosition())
		* m_pMobile->getHolder()->getMaxSpeed();

	return (DesiredVelocity - m_pMobile->getHolder()->getVelocity());
}

//----------------------------- Flee -------------------------------------
//
//  Does the opposite of Seek
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Flee(Vector2D TargetPos)
{
	//only flee if the target is within 'panic distance'. Work in distance
	//squared space.
	/* const double PanicDistanceSq = 100.0f * 100.0;
	if (Vec2DDistanceSq(m_pMobile->Pos(), target) > PanicDistanceSq)
	{
	return Vector2D(0,0);
	}
	*/

	Vector2D DesiredVelocity = Vec2DNormalize(m_pMobile->getHolder()->getPosition() - TargetPos) 
		* m_pMobile->getHolder()->getMaxSpeed();

	return (DesiredVelocity - m_pMobile->getHolder()->getVelocity());
}

//--------------------------- Arrive -------------------------------------
//
//  This behavior is similar to seek but it attempts to arrive at the
//  target with a zero velocity
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Arrive(Vector2D     TargetPos,
									Deceleration deceleration)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Arrive Holder is NULL");
	Vector2D ToTarget = TargetPos - m_pMobile->getHolder()->getPosition();
	//ToTarget.Accuracy(AS_VECTOR_ACCURACY);
	//calculate the distance to the target
	double distSQ = ToTarget.LengthSq();
	//if (distSQ>=AS_VECTOR_MIN&&distSQ>m_pMobile->getVelocity().LengthSq())
	{
		double dist = sqrt(distSQ);//ToTarget.Length();
		if (dist > 0)
		{
			//because Deceleration is enumerated as an int, this value is required
			//to provide fine tweaking of the deceleration..
			const double DecelerationTweaker = 0.3;

			//calculate the speed required to reach the target given the desired
			//deceleration
			double speed =  dist / ((double)deceleration * DecelerationTweaker);     

			//make sure the velocity does not exceed the max
			speed = MIN(speed, pHolder->getMaxSpeed());

			//from here proceed just like Seek except we don't need to normalize 
			//the ToTarget vector because we have already gone to the trouble
			//of calculating its length: dist. 
			Vector2D DesiredVelocity =  ToTarget * speed / dist;
			//ASLog("speed : %f",speed);
			//ASLog("DesireVelocity :(%f,%f), Velocity:(%f,%f)",DesiredVelocity.x,DesiredVelocity.y,m_pMobile->getVelocity().x,m_pMobile->getVelocity().y);
			return (DesiredVelocity - pHolder->getVelocity());
		}
	}
	//return Vector2D(0,0);
	return pHolder->getVelocity().GetReverse();
}

//------------------------------ Pursuit ---------------------------------
//
//  this behavior creates a force that steers the agent towards the 
//  evader
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Pursuit(ASEntity* evader)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Pursuit Holder is NULL");
	//if the evader is ahead and facing the agent then we can just seek
	//for the evader's current position.
	Vector2D ToEvader = evader->getPosition() - m_pMobile->getHolder()->getPosition();

	double RelativeHeading = pHolder->getHeading().Dot(evader->getHeading());

	if ( (ToEvader.Dot(pHolder->getHeading()) > 0) &&  
		(RelativeHeading < -0.95))  //acos(0.95)=18 degs
	{
		return Seek(evader->getPosition());
	}

	//Not considered ahead so we predict where the evader will be.

	//the lookahead time is propotional to the distance between the evader
	//and the pursuer; and is inversely proportional to the sum of the
	//agent's velocities
	double LookAheadTime = ToEvader.Length() / 
		(pHolder->getMaxSpeed() + evader->getSpeed());

	//now seek to the predicted future position of the evader
	return Seek(evader->getPosition() + evader->getVelocity() * LookAheadTime);
}


//----------------------------- Evade ------------------------------------
//
//  similar to pursuit except the agent Flees from the estimated future
//  position of the pursuer
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Evade(ASEntity* pursuer)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Evade Holder is NULL");
	/* Not necessary to include the check for facing direction this time */

	Vector2D ToPursuer = pursuer->getPosition() - pHolder->getPosition();

	//uncomment the following two lines to have Evade only consider pursuers 
	//within a 'threat range'
	const double ThreatRange = 100.0;
	if (ToPursuer.LengthSq() > ThreatRange * ThreatRange) return Vector2D();

	//the lookahead time is propotional to the distance between the pursuer
	//and the pursuer; and is inversely proportional to the sum of the
	//agents' velocities
	double LookAheadTime = ToPursuer.Length() / 
		(pHolder->getMaxSpeed() + pursuer->getSpeed());

	//now flee away from predicted future position of the pursuer
	return Flee(pHolder->getPosition() + pursuer->getVelocity() * LookAheadTime);
}


//--------------------------- Wander -------------------------------------
//
//  This behavior makes the agent wander about randomly
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::Wander()
//{ 
//  //this behavior is dependent on the update rate, so this line must
//  //be included when using time independent framerate.
//  double JitterThisTimeSlice = m_dWanderJitter * m_pMobile->TimeElapsed();
//
//  //first, add a small random vector to the target's position
//  m_vWanderTarget += Vector2D(RandomClamped() * JitterThisTimeSlice,
//                              RandomClamped() * JitterThisTimeSlice);
//
//  //reproject this new vector back on to a unit circle
//  m_vWanderTarget.Normalize();
//
//  //increase the length of the vector to the same as the radius
//  //of the wander circle
//  m_vWanderTarget *= m_dWanderRadius;
//
//  //move the target into a position WanderDist in front of the agent
//  Vector2D target = m_vWanderTarget + Vector2D(m_dWanderDistance, 0);
//
//  //project the target into world space
//  Vector2D Target = PointToWorldSpace(target,
//                                       m_pMobile->Heading(),
//                                       m_pMobile->Side(), 
//                                       m_pMobile->Pos());
//
//  //and steer towards it
//  return Target - m_pMobile->Pos(); 
//}
//

//---------------------- ObstacleAvoidance -------------------------------
//
//  Given a vector of CObstacles, this method returns a steering force
//  that will prevent the agent colliding with the closest obstacle
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::ObstacleAvoidance(const std::vector<ASEntity*>& obstacles)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::ObstacleAvoidance Holder is NULL");
	
	//the detection box length is proportional to the agent's velocity
	double dDBoxLength = pHolder->getBoundRadius() + 
		(pHolder->getSpeed()/pHolder->getMaxSpeed()) *
		pHolder->getBoundRadius();

	//tag all obstacles within range of the box for processing
	m_pMobile->getMobileControler()->tagObstacleWithinViewRange(m_pMobile->getHolder(), dDBoxLength);

	//this will keep track of the closest intersecting obstacle (CIB)
	ASEntity* ClosestIntersectingObstacle = NULL;
	//ASEntity* pHolder = m_pMobile->getHolder();
	//this will be used to track the distance to the CIB
	double DistToClosestIP = MaxDouble;

	//this will record the transformed local coordinates of the CIB
	Vector2D LocalPosOfClosestObstacle;

	std::vector<ASEntity*>::const_iterator curOb = obstacles.begin();

	while(curOb != obstacles.end())
	{
		//if the obstacle has been tagged within range proceed
		if ((*curOb)->isTaged()&&pHolder->checkCollideFlag((*curOb)))
		{
			//calculate this obstacle's position in local space
			Vector2D LocalPos = PointToLocalSpace((*curOb)->getPosition(),
				pHolder->getHeading(),
				pHolder->getSide(),
				pHolder->getPosition());

			//if the local position has a negative x value then it must lay
			//behind the agent. (in which case it can be ignored)
			if (LocalPos.x >= 0)
			{
				//if the distance from the x axis to the object's position is less
				//than its radius + half the width of the detection box then there
				//is a potential intersection.
				double ExpandedRadius = (*curOb)->getBoundRadius() + m_pMobile->getHolder()->getBoundRadius();

				if (fabs(LocalPos.y) < ExpandedRadius)
				{
					//now to do a line/circle intersection test. The center of the 
					//circle is represented by (cX, cY). The intersection points are 
					//given by the formula x = cX +/-sqrt(r^2-cY^2) for y=0. 
					//We only need to look at the smallest positive value of x because
					//that will be the closest point of intersection.
					double cX = LocalPos.x;
					double cY = LocalPos.y;

					//we only need to calculate the sqrt part of the above equation once
					double SqrtPart = sqrt(ExpandedRadius*ExpandedRadius - cY*cY);

					double ip = cX - SqrtPart;

					if (ip <= 0.0)
					{
						ip = cX + SqrtPart;
					}

					//test to see if this is the closest so far. If it is keep a
					//record of the obstacle and its local coordinates
					if (ip < DistToClosestIP)
					{
						DistToClosestIP = ip;

						ClosestIntersectingObstacle = *curOb;

						LocalPosOfClosestObstacle = LocalPos;
					}         
				}
			}
		}

		++curOb;
	}

	//if we have found an intersecting obstacle, calculate a steering 
	//force away from it
	Vector2D SteeringForce;

	if (ClosestIntersectingObstacle)
	{
		//the closer the agent is to an object, the stronger the 
		//steering force should be
		double multiplier = 1.0 + (dDBoxLength - LocalPosOfClosestObstacle.x) /
			dDBoxLength;

		//calculate the lateral force
		SteeringForce.y = (ClosestIntersectingObstacle->getBoundRadius()-
			LocalPosOfClosestObstacle.y)  * multiplier;   

		//apply a braking force proportional to the obstacles distance from
		//the vehicle. 
		const double BrakingWeight = 0.2;

		SteeringForce.x = (ClosestIntersectingObstacle->getBoundRadius() - 
			LocalPosOfClosestObstacle.x) * 
			BrakingWeight;
	}

	//finally, convert the steering vector from local to world space
	return VectorToWorldSpace(SteeringForce,
		pHolder->getHeading(),
		pHolder->getSide());
}


//--------------------------- WallAvoidance --------------------------------
//
//  This returns a steering force that will keep the agent away from any
//  walls it may encounter
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::WallAvoidance(const std::vector<ASWall2D>& walls)
{
	//the feelers are contained in a std::vector, m_Feelers
	CreateFeelers();

	double DistToThisIP    = 0.0;
	double DistToClosestIP = MaxDouble;

	//this will hold an index into the vector of walls
	int ClosestWall = -1;

	Vector2D SteeringForce,
		point,         //used for storing temporary info
		ClosestPoint;  //holds the closest intersection point

	//examine each feeler in turn
	for (unsigned int flr=0; flr<m_Feelers.size(); ++flr)
	{
		//run through each wall checking for any intersection points
		for (unsigned int w=0; w<walls.size(); ++w)
		{
			if (LineIntersection2D(m_pMobile->getHolder()->getPosition(),
				m_Feelers[flr],
				walls[w].From(),
				walls[w].To(),
				DistToThisIP,
				point))
			{
				//is this the closest found so far? If so keep a record
				if (DistToThisIP < DistToClosestIP)
				{
					DistToClosestIP = DistToThisIP;

					ClosestWall = w;

					ClosestPoint = point;
				}
			}
		}//next wall


		//if an intersection point has been detected, calculate a force  
		//that will direct the agent away
		if (ClosestWall >=0)
		{
			//calculate by what distance the projected position of the agent
			//will overshoot the wall
			Vector2D OverShoot = m_Feelers[flr] - ClosestPoint;

			//create a force in the direction of the wall normal, with a 
			//magnitude of the overshoot
			SteeringForce = walls[ClosestWall].Normal() * OverShoot.Length();
		}

	}//next feeler

	return SteeringForce;
}

//------------------------------- CreateFeelers --------------------------
//
//  Creates the antenna utilized by WallAvoidance
//------------------------------------------------------------------------
void ASSteeringBehavior::CreateFeelers()
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::CreateFeelers Holder is NULL");
	//feeler pointing straight in front
	m_Feelers[0] = m_pMobile->getHolder()->getPosition() + m_dWallDetectionFeelerLength * pHolder->getHeading();

	//feeler to left
	Vector2D temp = pHolder->getHeading();
	Vec2DRotateAroundOrigin(temp, HalfPi * 3.5f);
	m_Feelers[1] = m_pMobile->getHolder()->getPosition() + m_dWallDetectionFeelerLength/2.0f * temp;

	//feeler to right
	temp = pHolder->getHeading();
	Vec2DRotateAroundOrigin(temp, HalfPi * 0.5f);
	m_Feelers[2] = m_pMobile->getHolder()->getPosition() + m_dWallDetectionFeelerLength/2.0f * temp;
}


//---------------------------- Separation --------------------------------
//
// this calculates a force repelling from the other neighbors
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Separation(const std::vector<ASEntity*> &neighbors)
{  
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Separation Holder is NULL");
	Vector2D SteeringForce;
	for (unsigned int a=0; a<neighbors.size(); ++a)
	{
		//make sure this agent isn't included in the calculations and that
		//the agent being examined is close enough. ***also make sure it doesn't
		//include the evade target ***
		if((neighbors[a] != pHolder) && neighbors[a]->isTaged() && pHolder->checkCollideFlag(neighbors[a]))
		{
			Vector2D ToAgent = pHolder->getPosition() - neighbors[a]->getPosition();
			double length = ToAgent.Length();
			if (length>0.000001)
			{
				ToAgent/=length;
			}
			else
			{
				ToAgent = pHolder->getSide();
			}
			//scale the force inversely proportional to the agents distance  
			//from its neighbor.
			//SteeringForce += Vec2DNormalize(ToAgent)/ToAgent.Length();
			SteeringForce += ToAgent*(pHolder->getBoundRadius()+neighbors[a]->getBoundRadius()-length);
		}
	}

	return SteeringForce;
}

Vector2D ASSteeringBehavior::ForwardResistance(const std::vector<ASEntity*> &neighbors)
{  
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::ForwardResistance Holder is NULL");
	Vector2D SteeringForce;
	int count = 0;
	for (unsigned int a=0; a<neighbors.size(); ++a)
	{
		//make sure this agent isn't included in the calculations and that
		//the agent being examined is close enough. ***also make sure it doesn't
		//include the evade target ***
		if((neighbors[a] != pHolder) && neighbors[a]->isTaged() && pHolder->checkCollideFlag(neighbors[a]))
		{
			if ((neighbors[a]->getAlignOrder()<pHolder->getAlignOrder())||(neighbors[a]->getAlignOrder()==pHolder->getAlignOrder()&&neighbors[a]->getASObjectID()<m_pMobile->getASObjectID()))
			{
				Vector2D ToAgent = neighbors[a]->getPosition() - m_pMobile->getHolder()->getPosition();
				double dis = ToAgent.Length();
				Vector2D ToAgentNormal = dis>AS_VECTOR_MIN?ToAgent/dis:pHolder->getHeading();

				if ( (ToAgentNormal.Dot(pHolder->getHeading()) > 0.7) &&  neighbors[a]->getHeading().Dot(pHolder->getHeading()) > 0.7)
				{
					int checkdis = pHolder->getBoundRadius()+neighbors[a]->getBoundRadius();
					dis = checkdis-dis;
					dis = dis*dis*checkdis/(checkdis*checkdis);
					if(dis>0)
					{
						count++;
						SteeringForce += (ToAgentNormal.GetReverse()*dis);
					}
				}
				//scale the force inversely proportional to the agents distance  
				//from its neighbor.
				//SteeringForce += Vec2DNormalize(ToAgent)/ToAgent.Length();
				//SteeringForce += ToAgent;
			}
		}
	}
	//if (count>1)
	//{
	//	SteeringForce/=count;
	//}
	ASLog("Count:%d, Froce(%f,%f)",count,SteeringForce.x,SteeringForce.y);
	return SteeringForce;
}


//---------------------------- Alignment ---------------------------------
//
//  returns a force that attempts to align this agents heading with that
//  of its neighbors
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Alignment(const std::vector<ASEntity*>& neighbors)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Alignment Holder is NULL");
	//used to record the average heading of the neighbors
	Vector2D AverageHeading;

	//used to count the number of vehicles in the neighborhood
	int    NeighborCount = 0;

	//iterate through all the tagged vehicles and sum their heading vectors  
	for (unsigned int a=0; a<neighbors.size(); ++a)
	{
		//make sure *this* agent isn't included in the calculations and that
		//the agent being examined  is close enough ***also make sure it doesn't
		//include any evade target ***
		if((neighbors[a] != pHolder) && neighbors[a]->isTaged() &&
			(neighbors[a] != m_pTargetAgent1))
		{
			AverageHeading += neighbors[a]->getHeading();

			++NeighborCount;
		}
	}

	//if the neighborhood contained one or more vehicles, average their
	//heading vectors.
	if (NeighborCount > 0)
	{
		AverageHeading /= (double)NeighborCount;

		AverageHeading -= pHolder->getHeading();
	}

	return AverageHeading;
}

//-------------------------------- Cohesion ------------------------------
//
//  returns a steering force that attempts to move the agent towards the
//  center of mass of the agents in its immediate area
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Cohesion(const std::vector<ASEntity*> &neighbors)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Cohesion Holder is NULL");
	//first find the center of mass of all the agents
	Vector2D CenterOfMass, SteeringForce;

	int NeighborCount = 0;

	//iterate through the neighbors and sum up all the position vectors
	for (unsigned int a=0; a<neighbors.size(); ++a)
	{
		//make sure *this* agent isn't included in the calculations and that
		//the agent being examined is close enough ***also make sure it doesn't
		//include the evade target ***
		if((neighbors[a] != pHolder) && neighbors[a]->isTaged() &&
			(neighbors[a] != m_pTargetAgent1))
		{
			CenterOfMass += neighbors[a]->getPosition();

			++NeighborCount;
		}
	}

	if (NeighborCount > 0)
	{
		//the center of mass is the average of the sum of positions
		CenterOfMass /= (double)NeighborCount;

		//now seek towards that position
		SteeringForce = Seek(CenterOfMass);
	}

	//the magnitude of cohesion is usually much larger than separation or
	//allignment so it usually helps to normalize it.
	return Vec2DNormalize(SteeringForce);
}


/* NOTE: the next three behaviors are the same as the above three, except
that they use a cell-space partition to find the neighbors
*/


//---------------------------- Separation --------------------------------
//
// this calculates a force repelling from the other neighbors
//
//  USES SPACIAL PARTITIONING
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::SeparationPlus(const std::vector<ASMobileComponent*> &neighbors)
//{  
//  Vector2D SteeringForce;
//
//  //iterate through the neighbors and sum up all the position vectors
//  for (BaseGameEntity* pV = m_pMobile->World()->CellSpace()->begin();
//                         !m_pMobile->World()->CellSpace()->end();     
//                       pV = m_pMobile->World()->CellSpace()->next())
//  {    
//    //make sure this agent isn't included in the calculations and that
//    //the agent being examined is close enough
//    if(pV != m_pMobile)
//    {
//      Vector2D ToAgent = m_pMobile->Pos() - pV->Pos();
//
//      //scale the force inversely proportional to the agents distance  
//      //from its neighbor.
//      SteeringForce += Vec2DNormalize(ToAgent)/ToAgent.Length();
//    }
//
//  }
//
//  return SteeringForce;
//}
//---------------------------- Alignment ---------------------------------
//
//  returns a force that attempts to align this agents heading with that
//  of its neighbors
//
//  USES SPACIAL PARTITIONING
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::AlignmentPlus(const std::vector<ASMobileComponent*> &neighbors)
//{
//  //This will record the average heading of the neighbors
//  Vector2D AverageHeading;
//
//  //This count the number of vehicles in the neighborhood
//  double    NeighborCount = 0.0;
//
//  //iterate through the neighbors and sum up all the position vectors
//  for (MovingEntity* pV = m_pMobile->World()->CellSpace()->begin();
//                         !m_pMobile->World()->CellSpace()->end();     
//                     pV = m_pMobile->World()->CellSpace()->next())
//  {
//    //make sure *this* agent isn't included in the calculations and that
//    //the agent being examined  is close enough
//    if(pV != m_pMobile)
//    {
//      AverageHeading += pV->Heading();
//
//      ++NeighborCount;
//    }
//
//  }
//
//  //if the neighborhood contained one or more vehicles, average their
//  //heading vectors.
//  if (NeighborCount > 0.0)
//  {
//    AverageHeading /= NeighborCount;
//
//    AverageHeading -= m_pMobile->Heading();
//  }
//  
//  return AverageHeading;
//}


//-------------------------------- Cohesion ------------------------------
//
//  returns a steering force that attempts to move the agent towards the
//  center of mass of the agents in its immediate area
//
//  USES SPACIAL PARTITIONING
//------------------------------------------------------------------------
//Vector2D ASSteeringBehavior::CohesionPlus(const std::vector<ASMobileComponent*> &neighbors)
//{
//  //first find the center of mass of all the agents
//  Vector2D CenterOfMass, SteeringForce;
//
//  int NeighborCount = 0;
//
//  //iterate through the neighbors and sum up all the position vectors
//  for (BaseGameEntity* pV = m_pMobile->World()->CellSpace()->begin();
//                         !m_pMobile->World()->CellSpace()->end();     
//                       pV = m_pMobile->World()->CellSpace()->next())
//  {
//    //make sure *this* agent isn't included in the calculations and that
//    //the agent being examined is close enough
//    if(pV != m_pMobile)
//    {
//      CenterOfMass += pV->Pos();
//
//      ++NeighborCount;
//    }
//  }
//
//  if (NeighborCount > 0)
//  {
//    //the center of mass is the average of the sum of positions
//    CenterOfMass /= (double)NeighborCount;
//
//    //now seek towards that position
//    SteeringForce = Seek(CenterOfMass);
//  }
//
//  //the magnitude of cohesion is usually much larger than separation or
//  //allignment so it usually helps to normalize it.
//  return Vec2DNormalize(SteeringForce);
//}


//--------------------------- Interpose ----------------------------------
//
//  Given two agents, this method returns a force that attempts to 
//  position the vehicle between them
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Interpose(ASEntity* AgentA,
									   ASEntity* AgentB)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Interpose Holder is NULL");
	//first we need to figure out where the two agents are going to be at 
	//time T in the future. This is approximated by determining the time
	//taken to reach the mid way point at the current time at at max speed.
	Vector2D MidPoint = (AgentA->getPosition() + AgentB->getPosition()) / 2.0;

	double TimeToReachMidPoint = Vec2DDistance(m_pMobile->getHolder()->getPosition(), MidPoint) /
		pHolder->getMaxSpeed();

	//now we have T, we assume that agent A and agent B will continue on a
	//straight trajectory and extrapolate to get their future positions
	Vector2D APos = AgentA->getPosition() + AgentA->getVelocity() * TimeToReachMidPoint;
	Vector2D BPos = AgentB->getPosition() + AgentB->getVelocity() * TimeToReachMidPoint;

	//calculate the mid point of these predicted positions
	MidPoint = (APos + BPos) / 2.0;

	//then steer to Arrive at it
	return Arrive(MidPoint, fast);
}

//--------------------------- Hide ---------------------------------------
//
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::Hide(ASEntity*           hunter,
								  const std::vector<ASEntity*>& obstacles)
{
	double    DistToClosest = MaxDouble;
	Vector2D BestHidingSpot;

	std::vector<ASEntity*>::const_iterator curOb = obstacles.begin();
	std::vector<ASEntity*>::const_iterator closest;

	while(curOb != obstacles.end())
	{
		//calculate the position of the hiding spot for this obstacle
		Vector2D HidingSpot = GetHidingPosition((*curOb)->getPosition(),
			(*curOb)->getBoundRadius(),
			hunter->getPosition());

		//work in distance-squared space to find the closest hiding
		//spot to the agent
		double dist = Vec2DDistanceSq(HidingSpot, m_pMobile->getHolder()->getPosition());

		if (dist < DistToClosest)
		{
			DistToClosest = dist;

			BestHidingSpot = HidingSpot;

			closest = curOb;
		}  

		++curOb;

	}//end while

	//if no suitable obstacles found then Evade the hunter
	if (DistToClosest == MaxFloat)
	{
		return Evade(hunter);
	}

	//else use Arrive on the hiding spot
	return Arrive(BestHidingSpot, fast);
}

//------------------------- GetHidingPosition ----------------------------
//
//  Given the position of a hunter, and the position and radius of
//  an obstacle, this method calculates a position DistanceFromBoundary 
//  away from its bounding radius and directly opposite the hunter
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::GetHidingPosition(const Vector2D& posOb,
											   const double     radiusOb,
											   const Vector2D& posHunter)
{
	//calculate how far away the agent is to be from the chosen obstacle's
	//bounding radius
	const double DistanceFromBoundary = 30.0;
	double       DistAway    = radiusOb + DistanceFromBoundary;

	//calculate the heading toward the object from the hunter
	Vector2D ToOb = Vec2DNormalize(posOb - posHunter);

	//scale it to size and add to the obstacles position to get
	//the hiding spot.
	return (ToOb * DistAway) + posOb;
}


//------------------------------- FollowPath -----------------------------
//
//  Given a series of Vector2Ds, this method produces a force that will
//  move the agent along the waypoints in order. The agent uses the
// 'Seek' behavior to move to the next waypoint - unless it is the last
//  waypoint, in which case it 'Arrives'
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::FollowPath()
{
	if (!m_pPath->Finished())
	{
		//move to next target if close enough to current target (working in
		//distance squared space)
		if(Vec2DDistanceSq(m_pPath->CurrentWaypoint(), m_pMobile->getHolder()->getPosition()) <
			m_dWaypointSeekDistSq)
		{
			m_pPath->SetNextWaypoint();
		}
		if (m_pPath->IsLastWaypoint())
		{
			return Arrive(m_pPath->CurrentWaypoint(), normal);
		}
		else
		{
			return Seek(m_pPath->CurrentWaypoint());
			//return Arrive(m_pPath->CurrentWaypoint(), normal);
		}
	}
	else
	{//finish
		return Arrive(m_pPath->CurrentWaypoint(), normal);
	}
}

Vector2D ASSteeringBehavior::CrowdPath(const std::vector<ASEntity*> &neighbors,float dt)
{
	if (!m_pPath->Finished())
	{
		ASEntity* pHolder = m_pMobile->getHolder();
		ASAssert(pHolder!=nullptr,"ASSteeringBehavior::CrowdPath Holder is NULL");
		//move to next target if close enough to current target (working in
		//distance squared space)
		double nextPointCheckDisSQ = MaxOf((pHolder->getVelocity()*dt).LengthSq(),m_dWaypointSeekDistSq);
		//if(Vec2DDistanceSq(m_pPath->CurrentWaypoint(), m_pMobile->getHolder()->getPosition()) < m_dWaypointSeekDistSq)
		if(Vec2DDistanceSq(m_pPath->CurrentWaypoint(), m_pMobile->getHolder()->getPosition()) < nextPointCheckDisSQ)
		{
			m_pPath->SetNextWaypoint();
		}
		if (m_pPath->IsLastWaypoint())
		{
			return Arrive(m_pPath->CurrentWaypoint(), normal);
		}
		else if (!m_pPath->Finished())
		{
			Vector2D force = Seek(m_pPath->CurrentWaypoint());
			//ForwardResistance(m_pMobile->getMobileControler()->getMobiles());
			double forcelenSQ = force.LengthSq();
			if (forcelenSQ>0.000001)
			{
#if 1
				float coefficient = 1.0;
				//double forcelength = sqrt(forcelenSQ);
				//Vector2D forceNormal = force/forcelength;
				ASEntity* pHolder = m_pMobile->getHolder();
				Vector2D holderHeading = pHolder->getHeading();
				double holderVelSQ = pHolder->getVelocity().LengthSq();
				int holderBoundRadius = pHolder->getBoundRadius()*2;
				for (unsigned int a=0; a<neighbors.size(); ++a)
				{
					if((neighbors[a] != pHolder) && neighbors[a]->isTaged() && pHolder->checkCollideFlag(neighbors[a]))
					{
						if ((neighbors[a]->getAlignOrder()<pHolder->getAlignOrder())||(neighbors[a]->getAlignOrder()==pHolder->getAlignOrder()&&neighbors[a]->getASObjectID()<m_pMobile->getASObjectID()))
						{
							if(neighbors[a]->getVelocity().LengthSq()>=holderVelSQ&&holderHeading.Dot(neighbors[a]->getVelocity())>0.7)
							{
								Vector2D ToAgent = neighbors[a]->getPosition() - m_pMobile->getHolder()->getPosition();
								double disSQ = ToAgent.LengthSq();
							
								int checkdis = holderBoundRadius+neighbors[a]->getBoundRadius();
								int checkdisSQ = checkdis*checkdis;
								if (checkdisSQ>disSQ)
								{
									coefficient *= disSQ/checkdisSQ;
									//coefficient *= coefficient;
								}
							}
						}
					}
				}
				force = force*coefficient;
#else
				double forcelength = sqrt(forcelenSQ);
				Vector2D forceNormal = force/forcelength;
				ASEntity* pHolder = m_pMobile->getHolder();
				for (unsigned int a=0; a<neighbors.size(); ++a)
				{
					if((neighbors[a] != pHolder) && neighbors[a]->isTaged() && pHolder->checkCollideFlag(neighbors[a]))
					{
						if ((neighbors[a]->getAlignOrder()<pHolder->getAlignOrder())||(neighbors[a]->getAlignOrder()==pHolder->getAlignOrder()&&neighbors[a]->getASObjectID()<m_pMobile->getASObjectID()))
						{
							Vector2D ToAgent = neighbors[a]->getPosition() - m_pMobile->getHolder()->getPosition();
							double dis = ToAgent.Length();
							Vector2D ToAgentNormal = dis>AS_VECTOR_MIN?ToAgent/dis:pHolder->getHeading();
							if ( (ToAgentNormal.Dot(forceNormal) > 0.7) &&  neighbors[a]->getHeading().Dot(forceNormal) > 0.7)
							{
								int checkdis = pHolder->getBoundRadius()+neighbors[a]->getBoundRadius();
								dis = checkdis-dis;
								dis = dis*forcelength*2/checkdis;
								if(dis>0)
								{
									forcelength -= dis;
									if (forcelength<0.001)
									{
										forcelength = 0.001;
										break;
									}
								}
							}
						}
					}
				}
				force = forceNormal*forcelength;
#endif
			}
			return force;
		}
	}
	else
	{//finish
		return Arrive(m_pPath->CurrentWaypoint(), normal);
	}
}

//------------------------- Offset Pursuit -------------------------------
//
//  Produces a steering force that keeps a vehicle at a specified offset
//  from a leader vehicle
//------------------------------------------------------------------------
Vector2D ASSteeringBehavior::OffsetPursuit(ASEntity*  leader,
										   const Vector2D offset)
{
	//calculate the offset's position in world space
	Vector2D WorldOffsetPos = PointToWorldSpace(offset,
		leader->getHeading(),
		leader->getSide(),
		leader->getPosition());

	Vector2D ToOffset = WorldOffsetPos - m_pMobile->getHolder()->getPosition();

	//the lookahead time is propotional to the distance between the leader
	//and the pursuer; and is inversely proportional to the sum of both
	//agent's velocities
	//double LookAheadTime = ToOffset.Length() / (m_pMobile->getMaxSpeed() + leader->getSpeed());
	//Vector2D aim = WorldOffsetPos + leader->getVelocity() * LookAheadTime;
	Vector2D aim = WorldOffsetPos + leader->getVelocity();
	//aim.Accuracy(AS_VECTOR_ACCURACY);
	//now Arrive at the predicted future position of the offset
	return Arrive(aim, fast);
}

Vector2D ASSteeringBehavior::ApproachHorizon(ASEntity* pTarget,int range,float dt)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::ApproachHorizon Holder is NULL");
	double holderradius = pHolder->getBoundRadius()+pHolder->getAttackRadius()*0.5;
	Vector2D Aim = pTarget->getPosition()+Vector2D(((pTarget->getPosition().x<pHolder->getPosition().x)?1:-1)*(holderradius+pTarget->getBoundRadius()),0);
	Aim+=pTarget->getVelocity()*dt;
	Vector2D ToTarget = Aim - m_pMobile->getHolder()->getPosition();
	double lengthSQ = ToTarget.LengthSq();
	if (lengthSQ>0)
	{
		double checkForwardDistance = pHolder->getAttackRadius()*0.5;//holderradius+((pHolder->getBoundRadius()>>1)+AS_VECTOR_MIN);
		if (lengthSQ > checkForwardDistance*checkForwardDistance)
		{
			return ToTarget;
		}
		//double checkStopDistance = holderradius+((pHolder->getBoundRadius()>>2)+AS_VECTOR_MIN);
		//if (lengthSQ < checkStopDistance*checkStopDistance)
		//{
		//	return Vector2D(0,0);
		//}
	}
	//return pHolder->getVelocity().GetReverse();
	return Vector2D(0,0);
}

Vector2D ASSteeringBehavior::Approach(ASEntity* pTarget,int range)
{
	ASEntity* pHolder = m_pMobile->getHolder();
	ASAssert(pHolder!=nullptr,"ASSteeringBehavior::Approach Holder is NULL");
	Vector2D ToTarget = pTarget->getPosition() - m_pMobile->getHolder()->getPosition();
	Vector2D ToTargetNormal = pHolder->getHeading();
	double length = ToTarget.Length();
	if (length>0.000001)
	{
		ToTargetNormal = ToTarget/length;
	}
	pHolder->setHeading(ToTargetNormal);
	int minRadius = pHolder->getBoundRadius()+pTarget->getBoundRadius();
	int maxRadius = pHolder->getBoundRadius()+pTarget->getBoundRadius()+range;
	Vector2D force;
	if (length>maxRadius)
	{
		force = ToTargetNormal*(length-maxRadius)-pHolder->getVelocity();
		//force += (force.Perp()*0.5);
		return force;
	}
	else if (length<minRadius)
	{
		force = ToTargetNormal*(length-minRadius)-pHolder->getVelocity();
		//force += (force.Perp()*0.5);
		return force;
	}
	
	return pHolder->getVelocity().GetReverse();//Vector2D(0,0);
}

//for receiving keyboard input from user
//#define KEYDOWN(vk_code) ((GetAsyncKeyState(vk_code) & 0x8000) ? 1 : 0)
//----------------------------- RenderAids -------------------------------
//
//------------------------------------------------------------------------
//void ASSteeringBehavior::RenderAids( )
//{ 
//  
//  gdi->TransparentText();
//  gdi->TextColor(Cgdi::grey);
//
//  int NextSlot = 0; int SlotSize = 20;
//
//  if (KEYDOWN(VK_INSERT)){m_pMobile->SetMaxForce(m_pMobile->MaxForce() + 1000.0f*m_pMobile->TimeElapsed());} 
//  if (KEYDOWN(VK_DELETE)){if (m_pMobile->MaxForce() > 0.2f) m_pMobile->SetMaxForce(m_pMobile->MaxForce() - 1000.0f*m_pMobile->TimeElapsed());}
//  if (KEYDOWN(VK_HOME)){m_pMobile->SetMaxSpeed(m_pMobile->MaxSpeed() + 50.0f*m_pMobile->TimeElapsed());}
//  if (KEYDOWN(VK_END)){if (m_pMobile->MaxSpeed() > 0.2f) m_pMobile->SetMaxSpeed(m_pMobile->MaxSpeed() - 50.0f*m_pMobile->TimeElapsed());}
//
//  if (m_pMobile->MaxForce() < 0) m_pMobile->SetMaxForce(0.0f);
//  if (m_pMobile->MaxSpeed() < 0) m_pMobile->SetMaxSpeed(0.0f);
//  
//  if (m_pMobile->ID() == 0){ gdi->TextAtPos(5,NextSlot,"MaxForce(Ins/Del):"); gdi->TextAtPos(160,NextSlot,ttos(m_pMobile->MaxForce()/Prm.SteeringForceTweaker)); NextSlot+=SlotSize;}
//  if (m_pMobile->ID() == 0){ gdi->TextAtPos(5,NextSlot,"MaxSpeed(Home/End):"); gdi->TextAtPos(160,NextSlot,ttos(m_pMobile->MaxSpeed()));NextSlot+=SlotSize;}
//
//  //render the steering force
//  if (m_pMobile->World()->RenderSteeringForce())
//  {  
//    gdi->RedPen();
//    Vector2D F = (m_vSteeringForce / Prm.SteeringForceTweaker) * Prm.VehicleScale ;
//    gdi->Line(m_pMobile->Pos(), m_pMobile->Pos() + F);
//  }
//
//  //render wander stuff if relevant
//  if (On(wander) && m_pMobile->World()->RenderWanderCircle())
//  {    
//    if (KEYDOWN('F')){m_dWanderJitter+=1.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderJitter, 0.0f, 100.0f);}
//    if (KEYDOWN('V')){m_dWanderJitter-=1.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderJitter, 0.0f, 100.0f );}
//    if (KEYDOWN('G')){m_dWanderDistance+=2.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderDistance, 0.0f, 50.0f);}
//    if (KEYDOWN('B')){m_dWanderDistance-=2.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderDistance, 0.0f, 50.0f);}
//    if (KEYDOWN('H')){m_dWanderRadius+=2.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderRadius, 0.0f, 100.0f);}
//    if (KEYDOWN('N')){m_dWanderRadius-=2.0f*m_pMobile->TimeElapsed(); Clamp(m_dWanderRadius, 0.0f, 100.0f);}
//
// 
//    if (m_pMobile->ID() == 0){ gdi->TextAtPos(5,NextSlot, "Jitter(F/V): "); gdi->TextAtPos(160, NextSlot, ttos(m_dWanderJitter));NextSlot+=SlotSize;}
//    if (m_pMobile->ID() == 0) {gdi->TextAtPos(5,NextSlot,"Distance(G/B): "); gdi->TextAtPos(160, NextSlot, ttos(m_dWanderDistance));NextSlot+=SlotSize;}
//    if (m_pMobile->ID() == 0) {gdi->TextAtPos(5,NextSlot,"Radius(H/N): ");gdi->TextAtPos(160, NextSlot,  ttos(m_dWanderRadius));NextSlot+=SlotSize;}
//
//    
//    //calculate the center of the wander circle
//    Vector2D m_vTCC = PointToWorldSpace(Vector2D(m_dWanderDistance*m_pMobile->BRadius(), 0),
//                                         m_pMobile->Heading(),
//                                         m_pMobile->Side(),
//                                         m_pMobile->Pos());
//    //draw the wander circle
//    gdi->GreenPen();
//    gdi->HollowBrush();
//    gdi->Circle(m_vTCC, m_dWanderRadius*m_pMobile->BRadius()); 
//
//    //draw the wander target
//    gdi->RedPen();
//    gdi->Circle(PointToWorldSpace((m_vWanderTarget + Vector2D(m_dWanderDistance,0))*m_pMobile->BRadius(),
//                                  m_pMobile->Heading(),
//                                  m_pMobile->Side(),
//                                  m_pMobile->Pos()), 3);                                  
//  }
//
//  //render the detection box if relevant
//  if (m_pMobile->World()->RenderDetectionBox())
//  {
//    gdi->GreyPen();
//
//    //a vertex buffer rqd for drawing the detection box
//    static std::vector<Vector2D> box(4);
//
//    double length = Prm.MinDetectionBoxLength + 
//                  (m_pMobile->Speed()/m_pMobile->MaxSpeed()) *
//                  Prm.MinDetectionBoxLength;
//
//    //verts for the detection box buffer
//    box[0] = Vector2D(0,m_pMobile->BRadius());
//    box[1] = Vector2D(length, m_pMobile->BRadius());
//    box[2] = Vector2D(length, -m_pMobile->BRadius());
//    box[3] = Vector2D(0, -m_pMobile->BRadius());
// 
//  
//    if (!m_pMobile->isSmoothingOn())
//    {
//      box = WorldTransform(box,m_pMobile->Pos(),m_pMobile->Heading(),m_pMobile->Side());
//      gdi->ClosedShape(box);
//    }
//    else
//    {
//      box = WorldTransform(box,m_pMobile->Pos(),m_pMobile->SmoothedHeading(),m_pMobile->SmoothedHeading().Perp());
//      gdi->ClosedShape(box);
//    } 
//
//
//    //////////////////////////////////////////////////////////////////////////
//    //the detection box length is proportional to the agent's velocity
//  m_dDBoxLength = Prm.MinDetectionBoxLength + 
//                  (m_pMobile->Speed()/m_pMobile->MaxSpeed()) *
//                  Prm.MinDetectionBoxLength;
//
//  //tag all obstacles within range of the box for processing
//  m_pMobile->World()->TagObstaclesWithinViewRange(m_pMobile, m_dDBoxLength);
//
//  //this will keep track of the closest intersecting obstacle (CIB)
//  BaseGameEntity* ClosestIntersectingObstacle = NULL;
// 
//  //this will be used to track the distance to the CIB
//  double DistToClosestIP = MaxDouble;
//
//  //this will record the transformed local coordinates of the CIB
//  Vector2D LocalPosOfClosestObstacle;
//
//  std::vector<BaseGameEntity*>::const_iterator curOb = m_pMobile->World()->Obstacles().begin();
//
//  while(curOb != m_pMobile->World()->Obstacles().end())
//  {
//    //if the obstacle has been tagged within range proceed
//    if ((*curOb)->IsTagged())
//    {
//      //calculate this obstacle's position in local space
//      Vector2D LocalPos = PointToLocalSpace((*curOb)->Pos(),
//                                             m_pMobile->Heading(),
//                                             m_pMobile->Side(),
//                                             m_pMobile->Pos());
//
//      //if the local position has a negative x value then it must lay
//      //behind the agent. (in which case it can be ignored)
//      if (LocalPos.x >= 0)
//      {
//        //if the distance from the x axis to the object's position is less
//        //than its radius + half the width of the detection box then there
//        //is a potential intersection.
//        if (fabs(LocalPos.y) < ((*curOb)->BRadius() + m_pMobile->BRadius()))
//        {
//          gdi->ThickRedPen();
//          gdi->ClosedShape(box);        
//        }
//      }
//    }
//
//    ++curOb;
//  }
//
//
///////////////////////////////////////////////////////
//  }
//
//  //render the wall avoidnace feelers
//  if (On(wall_avoidance) && m_pMobile->World()->RenderFeelers())
//  {
//    gdi->OrangePen();
//
//    for (unsigned int flr=0; flr<m_Feelers.size(); ++flr)
//    {
//
//      gdi->Line(m_pMobile->Pos(), m_Feelers[flr]);
//    }            
//  }  
//
//  //render path info
//  if (On(follow_path) && m_pMobile->World()->RenderPath())
//  {
//    m_pPath->Render();
//  }
//
//  
//  if (On(separation))
//  {
//    if (m_pMobile->ID() == 0){ gdi->TextAtPos(5, NextSlot, "Separation(S/X):");gdi->TextAtPos(160,NextSlot, ttos(m_dWeightSeparation/Prm.SteeringForceTweaker));NextSlot+=SlotSize;}
//
//    if (KEYDOWN('S')){m_dWeightSeparation += 200*m_pMobile->TimeElapsed(); Clamp(m_dWeightSeparation, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//    if (KEYDOWN('X')){m_dWeightSeparation -= 200*m_pMobile->TimeElapsed();Clamp(m_dWeightSeparation, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//  }
//
//  if (On(allignment))
//  {
//    if (m_pMobile->ID() == 0) {gdi->TextAtPos(5, NextSlot, "Alignment(A/Z):"); gdi->TextAtPos(160, NextSlot, ttos(m_dWeightAlignment/Prm.SteeringForceTweaker));NextSlot+=SlotSize;}
//
//    if (KEYDOWN('A')){m_dWeightAlignment += 200*m_pMobile->TimeElapsed();Clamp(m_dWeightAlignment, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//    if (KEYDOWN('Z')){m_dWeightAlignment -= 200*m_pMobile->TimeElapsed();Clamp(m_dWeightAlignment, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//  }
//
//  if (On(cohesion))
//  {
//    if (m_pMobile->ID() == 0) {gdi->TextAtPos(5, NextSlot, "Cohesion(D/C):"); gdi->TextAtPos(160, NextSlot, ttos(m_dWeightCohesion/Prm.SteeringForceTweaker));NextSlot+=SlotSize;}
//    if (KEYDOWN('D')){m_dWeightCohesion += 200*m_pMobile->TimeElapsed();Clamp(m_dWeightCohesion, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//    if (KEYDOWN('C')){m_dWeightCohesion -= 200*m_pMobile->TimeElapsed();Clamp(m_dWeightCohesion, 0.0f, 50.0f * Prm.SteeringForceTweaker);}
//  }
//
//  if (On(follow_path))
//  { 
//    double sd = sqrt(m_dWaypointSeekDistSq);
//    if (m_pMobile->ID() == 0){ gdi->TextAtPos(5, NextSlot, "SeekDistance(D/C):");gdi->TextAtPos(160, NextSlot,ttos(sd));NextSlot+=SlotSize;}
//    
//    if (KEYDOWN('D')){m_dWaypointSeekDistSq += 1.0;}
//    if (KEYDOWN('C')){m_dWaypointSeekDistSq -= 1.0; Clamp(m_dWaypointSeekDistSq, 0.0f, 400.0f);}
//  }  
//
//}

NS_AS_END



